#include "car_control.h"
extern double goal_point_x[100];              // 记录目标点的x位置
extern double goal_point_y[100];              // 记录目标点的y位置
extern double goal_point_z[100];              // 记录目标点的z位置
extern move_base_msgs::MoveBaseGoal cur_goal; // move_base的目标点位置
extern int count_goal;                        // 导航点数数量
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
using namespace westonrobot;
ScoutBase scout;

extern int Number_of_brakes;
extern int sidewalk_flag; // 未识别
extern int identify;
// MoveBaseClient ac("move_base", true);
void car_move(void) // 控制车自主导航的函数
{
    MoveBaseClient ac("move_base", true);
    while (!ac.waitForServer(ros::Duration(5.0)))
    {
        ROS_INFO("等待操作服务器报告它已出现并准备开始处理目标");
    } // 等待操作服务器报告它已出现并准备开始处理目标

    // 当前导航点
    int curPoint = 0;
    ROS_INFO_STREAM("目标点总共有 " << count_goal << "个");
    for (; curPoint < count_goal; curPoint++)
    {
        // 失败次数记录
        int try_times = 0;

        // 设置要发布的目标点信息
        cur_goal.target_pose.header.frame_id = "map";
        cur_goal.target_pose.header.stamp = ros::Time::now();
        cur_goal.target_pose.pose.position.x = goal_point_x[curPoint];
        cur_goal.target_pose.pose.position.y = goal_point_y[curPoint];
        cur_goal.target_pose.pose.position.z = 0.0;
        geometry_msgs::Quaternion q; // 定义四元数
        q = tf::createQuaternionMsgFromYaw(goal_point_z[curPoint] * 3.14 / 180);

        cur_goal.target_pose.pose.orientation.x = q.x;
        cur_goal.target_pose.pose.orientation.y = q.y;
        cur_goal.target_pose.pose.orientation.z = q.z;
        cur_goal.target_pose.pose.orientation.w = q.w;

        //        cur_goal.target_pose.pose.orientation = q;
        // 开始导航到当前目标点
        ac.sendGoal(cur_goal);
        while (true)
        {
            ROS_INFO_STREAM("去往目标点" << curPoint << " cur_goal X: " << cur_goal.target_pose.pose.position.x << " ,goal Y: " << cur_goal.target_pose.pose.position.y);
            ROS_INFO_STREAM("ac的状态 : " << ac.getState().toString());
            // 查看导航结果
            ac.waitForResult();
            // 成功到达目标点
            if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {

                ROS_INFO_STREAM("已经到达目标点: " << curPoint);
                // 先取消导航，让车停止一会
                ac.cancelAllGoals();
                ROS_INFO_STREAM("等待新的目标!  ");
                break;
            }
            else
            {
                // 尝试5次之后，导航失败
                if (try_times == 5)
                {
                    ROS_INFO("错误的目标点，可能它不在地图中，开始前往下一个目标");
                    // 取消这个导航点了，继续发布下一个点
                    ac.cancelAllGoals();
                    break;
                }
                ROS_INFO_STREAM("失败到达这个点，重新导航这个点试试" << curPoint << "  times : " << try_times);
                try_times++;
                // 取消这个点，然后重新导航这个点试试
                ac.cancelAllGoals();

                // 等待操作服务器报告它已出现并准备开始处理目标
                while (!ac.waitForServer(ros::Duration(5.0)))
                {
                    ROS_INFO("等待操作服务器报告它已出现并准备开始处理目标");
                }
                // 再次导航这个点
                ac.sendGoal(cur_goal);
            }
        }
    }
}
void get_goal(void) // 读取文件得到目标点的值
{
    ros::NodeHandle nh_private("~");
    // 8个目标点
    nh_private.param<int>("count_goal", count_goal, 8);

    nh_private.param<double>("x1", goal_point_x[0], 0.3); // 1
    nh_private.param<double>("y1", goal_point_y[0], 1.0);
    nh_private.param<double>("yaw1", goal_point_z[0], 1.0);

    nh_private.param<double>("x2", goal_point_x[1], 0.3); // 2
    nh_private.param<double>("y2", goal_point_y[1], 1.0);
    nh_private.param<double>("yaw2", goal_point_z[1], 1.0);

    nh_private.param<double>("x3", goal_point_x[2], 0.3); // 3
    nh_private.param<double>("y3", goal_point_y[2], 1.0);
    nh_private.param<double>("yaw3", goal_point_z[2], 1.0);

    nh_private.param<double>("x4", goal_point_x[3], 0.3); // 4
    nh_private.param<double>("y4", goal_point_y[3], 1.0);
    nh_private.param<double>("yaw4", goal_point_z[3], 1.0);

    nh_private.param<double>("x5", goal_point_x[4], 0.3); // 5
    nh_private.param<double>("y5", goal_point_y[4], 1.0);
    nh_private.param<double>("yaw5", goal_point_z[4], 1.0);

    nh_private.param<double>("x6", goal_point_x[5], 0.3); // 6
    nh_private.param<double>("y6", goal_point_y[5], 1.0);
    nh_private.param<double>("yaw6", goal_point_z[5], 1.0);

    nh_private.param<double>("x7", goal_point_x[6], 0.3); // 7
    nh_private.param<double>("y7", goal_point_y[6], 1.0);
    nh_private.param<double>("yaw7", goal_point_z[6], 1.0);

    nh_private.param<double>("x8", goal_point_x[7], 0.3); // 8
    nh_private.param<double>("y8", goal_point_y[7], 1.0);
    nh_private.param<double>("yaw8", goal_point_z[7], 1.0);

    nh_private.param<double>("x9", goal_point_x[8], 0.3); // 8
    nh_private.param<double>("y9", goal_point_y[8], 1.0);
    nh_private.param<double>("yaw9", goal_point_z[8], 1.0);
    // 打印目标点信息
    for (int i = 0; i < count_goal; i++)
    {

        ROS_INFO_STREAM("目标点[ " << i << "] : x: " << goal_point_x[i] << " y:  " << goal_point_y[i] << "  z: " << goal_point_z[i]);
    }
}
void base_init(void)
{
    std::string device_name = "can0";
    int32_t baud_rate = 0;
    scout.Connect(device_name, baud_rate); // 连接can
                                           //   scout.SetMotionCommand(0.2, 0.0);      // 设置角速度和线速度
}